/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package luft27.comm;

import java.util.Arrays;
import java.util.Comparator;
import java.util.HashMap;
import java.util.Map;
import luft27.comm.mavlink.*;
import luft27.propertytree.PropertyNode;
import luft27.propertytree.PropertyTree;
import luft27.propertytree.PropertyValue;
import luft27.util.RunnableLoop;

/**
 *
 * @author amalikov
 */
public class GCSLink extends RunnableLoop {

	public GCSLink(SerialStream s) {
		lastCycleTime = 0;
		
		mavLink = new ProtocolDebugAdapter(new Protocol(s), new byte[]{
			Id.HEARTBEAT, Id.GPS_RAW_INT, Id.MISSION_CURRENT, 
			Id.NAV_CONTROLLER_OUTPUT, Id.REQUEST_DATA_STREAM,	// filtered from debug output
			Id.ATTITUDE, Id.GLOBAL_POSITION_INT, Id.VFR_HUD,
			Id.SYS_STATUS});
		
		handlers = new HashMap<>();
		handlers.put(Id.PARAM_REQUEST_LIST, new ParamRequestListHandler());
		handlers.put(Id.PARAM_REQUEST_READ, new ParamRequestReadHandler());
		handlers.put(Id.PARAM_SET, new ParamSetHandler());
		handlers.put(Id.MISSION_REQUEST_LIST, new MissionRequestListHandler());
		handlers.put(Id.MISSION_REQUEST, new MissionRequestHandler());
		handlers.put(Id.MISSION_SET_CURRENT, new MissionSetCurrentHandler());
		handlers.put(Id.MISSION_CLEAR_ALL, new MissionClearAllHandler());
		handlers.put(Id.MISSION_COUNT, new MissionCountHandler());
		handlers.put(Id.MISSION_ITEM, new MissionItemHandler());
		handlers.put(Id.COMMAND_LONG, new CommandLongHandler());
	}	
	
	@Override
	public void init() {
	}

	@Override
	public void cycle() {
		Message msg = mavLink.recv();
		if (msg != null)
			handleMessage(msg);
		else {
			final long cycleTime = 50 * 1000000;
			if (System.nanoTime() - lastCycleTime  > cycleTime) {
				sendHeartBeat();
				sendSysStatus();
				sendAttitude();
				sendGlobalPositionInt();
				sendVfrHud();
				sendMissionCurrent();
				sendNavControllerOutput();
				sendGpsRawInt();
				lastCycleTime = System.nanoTime();
			}
		}
	}

	private void handleMessage(Message msg) {
		Handler h = handlers.get(msg.msgId);
		if (h != null)
			h.handle(msg.payload);
	}
	
	private void sendHeartBeat() {
		HeartBeat p = new HeartBeat();
		p.custom_mode = 0;
		p.type = Consts.MAV_TYPE_FIXED_WING;
		p.autopilot = Consts.MAV_AUTOPILOT_GENERIC;
		
		switch(PropertyTree.INSTANCE.get("/vehicles/vehicle[0]/state").asInt()) {
			case 0x01:
				p.base_mode = Consts.MAV_MODE_PREFLIGHT;
				break;
			case 0xC0:
			case 0xC1:
				p.base_mode = Consts.MAV_MODE_GUIDED_ARMED;
				break;
			case 0xC2:
				p.base_mode = Consts.MAV_MODE_MANUAL_ARMED;
				break;
			default:
				p.base_mode = Consts.MAV_MODE_AUTO_ARMED;
		}
		
		p.system_status = 0;
		p.mavlink_version = 3;
		mavLink.send(new Message(sysId, compId, Id.HEARTBEAT, p.get()));
	}

	private void sendSysStatus() {
		SysStatus p = new SysStatus();
		PropertyNode v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		p.onboard_control_sensors_present = 0;
		p.onboard_control_sensors_enabled = 0;
		p.onboard_control_sensors_health = 0;
		p.load = 50;
		p.voltage_battery = (short)(v.get("/electric/servo/uavg").asDouble() * 1000);
		p.current_battery = (short)(v.get("/electric/servo/itotal").asDouble() * 100);
		int goodCnt = PropertyTree.INSTANCE.get("/linkstatus/goodcnt").asInt();
		int errCnt = PropertyTree.INSTANCE.get("/linkstatus/errcnt").asInt();
		p.drop_rate_comm = (short)(errCnt / goodCnt * 100);
		p.errors_comm = (short)errCnt;
		p.errors_count1 = 0;
		p.errors_count2 = 0;
		p.errors_count3 = 0;
		p.errors_count4 = 0;
		p.battery_remaining = (byte)(v.get("/engine/fuel").asInt());
		mavLink.send(new Message(sysId, compId, Id.SYS_STATUS, p.get()));
	}
	
	private void sendAttitude() {
		Attitude p = new Attitude();
		p.time_boot_ms = (int)System.currentTimeMillis();
		PropertyNode v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		p.roll = (float)Math.toRadians(v.get("/orientation/roll").asDouble());
		p.pitch = (float)Math.toRadians(v.get("/orientation/pitch").asDouble());
		p.yaw = (float)Math.toRadians(v.get("/orientation/heading").asDouble());
		p.rollspeed = 0;
		p.pitchspeed = 0;
		p.yawspeed = 0;
		mavLink.send(new Message(sysId, compId, Id.ATTITUDE, p.get()));
	}
	
	private void sendGlobalPositionInt() {
		GlobalPositionInt p = new GlobalPositionInt();
		p.time_boot_ms = (int)System.currentTimeMillis();
		PropertyNode v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		p.lat = (int)(v.get("/position/latitude").asDouble() * 1e7);
		p.lon = (int)(v.get("/position/longitude").asDouble() * 1e7);
		p.alt = (int)(v.get("/position/altitude").asDouble() * 1e3);
		p.relative_alt = (int)(v.get("/airdata/altitude").asDouble() * 1e3);
		double gs = v.get("/position/gs").asDouble();
		double course = v.get("/position/course").asDouble();
		p.vx = (short)(gs * Math.cos(Math.toRadians(course)));
		p.vy = (short)(gs * Math.sin(Math.toRadians(course)));
		p.vz = (short)(v.get("/airdata/vs").asDouble());
		p.hdg = (short)(v.get("/orientation/heading").asDouble() * 100);
		mavLink.send(new Message(sysId, compId, Id.GLOBAL_POSITION_INT, p.get()));
	}

	private void sendVfrHud() {
		VfrHud p = new VfrHud();
		PropertyNode v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		p.airspeed = (float)v.get("/airdata/ias").asDouble();
		p.groundspeed = (float)v.get("/position/gs").asDouble();
		p.heading = (short)v.get("/orientation/heading").asDouble();
		p.throttle = (short)(Math.max(v.get("/controls/throttle").asDouble(), 0) * 100);
		p.alt = (float)v.get("/airdata/altitude").asDouble();
		p.alt = (float)v.get("/airdata/vs").asDouble();
		mavLink.send(new Message(sysId, compId, Id.VFR_HUD, p.get()));
	}

	private void sendMissionCurrent() {
		MissionCurrent p = new MissionCurrent();
		p.seq = (short)(PropertyTree.INSTANCE.get("/vehicles/vehicle[0]/track/waypointindex").asInt() + 1);
		mavLink.send(new Message(sysId, compId, Id.MISSION_CURRENT, p.get()));
	}
	
	private void sendNavControllerOutput() {
		NavControllerOutput p = new NavControllerOutput();
		PropertyNode v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		p.nav_roll = (float)v.get("/orientation/roll").asDouble();
		p.nav_pitch = (float)v.get("/orientation/pitch").asDouble();
		p.nav_bearing = (short)v.get("/position/course").asDouble();
		p.target_bearing = (short)v.get("/position/course").asDouble();
		p.wp_dist = (short)v.get("/track/s").asInt();
		p.alt_error = 0;
		p.aspd_error = 0;
		p.xtrack_error = (float)v.get("/track/z").asDouble();
		mavLink.send(new Message(sysId, compId, Id.NAV_CONTROLLER_OUTPUT, p.get()));
	}
	
	private void sendGpsRawInt() {
		GpsRawInt p = new GpsRawInt();
		PropertyNode v = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]");
		p.time_usec = 0;
		p.fix_type = 3;
		p.lat = (int)(v.get("/position/latitude").asDouble() * 1e7);
		p.lon = (int)(v.get("/position/longitude").asDouble() * 1e7);
		p.alt = (int)(v.get("/position/altitude").asDouble() * 1e3);
		p.eph = -1;
		p.epv = -1;
		p.vel = (short)(v.get("/position/gs").asDouble() * 100);
		p.cog = (short)(v.get("/position/course").asDouble() * 100);
		p.satellites_visible = 10;
		mavLink.send(new Message(sysId, compId, Id.GPS_RAW_INT, p.get()));
	}
	
	private interface Handler {
		void handle(byte[] reqPayload);
	}

	private class CommandLongHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			CommandLong rp = new CommandLong();
			rp.set(reqPayload);
			
			PropertyNode c = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/commands");
			switch(rp.command) {
			case Consts.MAV_CMD_NAV_LOITER_UNLIM:
				c.set("/auto", new PropertyValue(true));
				break;
			case Consts.MAV_CMD_NAV_RETURN_TO_LAUNCH:
				c.set("/abort", new PropertyValue(true));
				break;
			case Consts.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:	
				c.set("/emergency", new PropertyValue(true));
				break;				
			}

			CommandAck p = new CommandAck();
			p.command = rp.command;
			p.result = Consts.MAV_RESULT_ACCEPTED;
			mavLink.send(new Message(sysId, compId, Id.COMMAND_ACK, p.get()));
		}
	}
	
	private class ParamRequestListHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			PropertyNode[] params = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/params").getChildren();
			Arrays.sort(params, new Comparator<PropertyNode>() {
				@Override
				public int compare(PropertyNode o1, PropertyNode o2) {
					int i1 = o1.getIndex();
					int i2 = o2.getIndex();
					return i1 == i2 ? 0 : (i1 > i2 ? 1 : -1);
				}
			});
			
			for (PropertyNode param : params) {
				ParamValue p = new ParamValue();
				p.param_id = param.get("/name").asString();
				p.param_value = (float)param.get("/value").asDouble();
				p.param_type = ParamType.REAL32;
				p.param_index = (short)param.getIndex();
				p.param_count = (short)params.length;
				mavLink.send(new Message(sysId, compId, Id.PARAM_VALUE, p.get()));
			}
		}
	}

	private class ParamRequestReadHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			ParamRequestRead rp = new ParamRequestRead();
			rp.set(reqPayload);
			
			PropertyNode[] params = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/params").getChildren();
			PropertyNode param = findParamNode(params, rp.param_id, rp.param_index);
		
			if (param != null) {
				ParamValue p = new ParamValue();
				p.param_id = param.get("/name").asString();
				p.param_value = (float)param.get("/value").asDouble();
				p.param_type = ParamType.REAL32;
				p.param_index = (short)param.getIndex();
				p.param_count = (short)params.length;
				mavLink.send(new Message(sysId, compId, Id.PARAM_VALUE, p.get()));
			}
		}
	}

	private class ParamSetHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			ParamSet rp = new ParamSet();
			rp.set(reqPayload);

			PropertyNode[] params = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/params").getChildren();
			PropertyNode param = findParamNode(params, rp.param_id, -1);
			
			if (param != null) {
				param.set("/value", new PropertyValue(rp.param_value));
				param.set("/state", new PropertyValue(2));
			
				ParamValue p = new ParamValue();
				p.param_id = rp.param_id;
				p.param_value = rp.param_value;
				p.param_type = rp.param_type;
				p.param_index = (short)param.getIndex();
				p.param_count = (short)params.length;
				mavLink.send(new Message(sysId, compId, Id.PARAM_VALUE, p.get()));
			}
		}
	}
	
	private PropertyNode findParamNode(PropertyNode[] params, String name, int index) {
		for (PropertyNode param : params) {
			if (param.getIndex() == index || param.get("/name").asString().equals(name))
				return param;
		}		
		return null;
	}

	private class MissionRequestListHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			PropertyNode route = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/route");
			route.getNode("/new").removeChildren();
			
			int i = 0;
			boolean eof = false;
			while (!eof) {
				PropertyNode curwpt = route.getNode("/current/wpt[" + i + "]");
				PropertyNode newwpt = route.getNode("/new/wpt[" + i + "]");
				
				newwpt.set("/latitude", curwpt.get("/latitude"));
				newwpt.set("/longitude", curwpt.get("/longitude"));
				newwpt.set("/altitude", curwpt.get("/altitude"));
				newwpt.set("/present", curwpt.get("/present"));
				
				if (curwpt.get("/present").asBool() == false)
					eof = true;
				else
					++i;
			}

			MissionCount p = new MissionCount();
			p.target_system = 0;
			p.target_component = 0;			
			p.count = (short)(i + 1);	// plus home point
			
			mavLink.send(new Message(sysId, compId, Id.MISSION_COUNT, p.get()));
		}
	}

	private class MissionRequestHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			MissionRequest rp = new MissionRequest();
			rp.set(reqPayload);
			
			int i = Math.max(rp.seq - 1, 0);	// home point = first point
			PropertyNode wpt = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/route/current/wpt[" + i + "]");
			
			MissionItem p = new MissionItem();
			p.target_system = 0;
			p.target_component = 0;
			p.seq = rp.seq;
			p.current = 0;

			if (wpt.get("/present").asBool()) {
				p.autocontinue = 1;
				p.frame = Consts.MAV_FRAME_GLOBAL_RELATIVE_ALT;
				p.command = Consts.MAV_CMD_NAV_WAYPOINT;
				p.x = (float)wpt.get("/latitude").asDouble();
				p.y = (float)wpt.get("/longitude").asDouble();
				p.z = (float)wpt.get("/altitude").asDouble();
			} else {
				p.command = Consts.MAV_CMD_NAV_LAST;
			}
			
			mavLink.send(new Message(sysId, compId, Id.MISSION_ITEM, p.get()));
		}
	}

	private class MissionSetCurrentHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			MissionSetCurrent rp = new MissionSetCurrent();
			rp.set(reqPayload);
			
			int i = Math.max(rp.seq - 1, 0);	// home point = first point
			PropertyTree.INSTANCE.set("/vehicles/vehicle[0]/route/cmdTo", new PropertyValue(i));
			
			MissionCurrent p = new MissionCurrent();
			p.seq = rp.seq;
			mavLink.send(new Message(sysId, compId, Id.MISSION_CURRENT, p.get()));
		}
	}

	private class MissionClearAllHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			PropertyNode n = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/route/new");
			n.removeChildren();
			MissionAck p = new MissionAck();
			p.target_system = 0;
			p.target_component = 0;
			p.type = Consts.MAV_RESULT_ACCEPTED;
			mavLink.send(new Message(sysId, compId, Id.MISSION_ACK, p.get()));
		}
	}
	
	private class MissionCountHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			MissionCount rp = new MissionCount();
			rp.set(reqPayload);
			
			PropertyTree.INSTANCE.set("/vehicles/vehicle[0]/route/receiving", new PropertyValue(true));
			
			PropertyNode route = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/route/new");
			route.removeChildren();
			
			int n = Math.max(rp.count - 1, 0);	// minus home point
			
			for (int i = 0; i < n; ++i)
				route.set("/wpt[" + i + "]/present", new PropertyValue(true));
			
			route.set("/wpt[" + n + "]/present", new PropertyValue(false));
			
			MissionRequest p = new MissionRequest();
			p.target_system = 0;
			p.target_component = 0;
			p.seq = 0;
			mavLink.send(new Message(sysId, compId, Id.MISSION_REQUEST, p.get()));
		}
	}
	
	private class MissionItemHandler implements Handler {
		@Override
		public void handle(byte[] reqPayload) {
			MissionItem rp = new MissionItem();
			rp.set(reqPayload);
			
			PropertyNode route = PropertyTree.INSTANCE.getNode("/vehicles/vehicle[0]/route");
			
			if (route.get("/receiving").asBool()) {
				int i = Math.max(rp.seq - 1, 0);	// ignore home point
			
				if (rp.seq != 0) {	// ignore home point
					PropertyNode wpt = route.getNode("/new/wpt[" + i + "]");
					wpt.set("/latitude", new PropertyValue(rp.x));
					wpt.set("/longitude", new PropertyValue(rp.y));
					wpt.set("/altitude", new PropertyValue(rp.z));
				}
			
				if (route.get("/new/wpt[" + (i + 1) + "]/present").asBool() == true) {
					MissionRequest p = new MissionRequest();
					p.target_system = 0;
					p.target_component = 0;
					p.seq = (short)(rp.seq + 1);
					mavLink.send(new Message(sysId, compId, Id.MISSION_REQUEST, p.get()));
				} else {
					MissionAck p = new MissionAck();
					p.target_system = 0;
					p.target_component = 0;
					p.type = Consts.MAV_RESULT_ACCEPTED;
					mavLink.send(new Message(sysId, compId, Id.MISSION_ACK, p.get()));
				
					route.set("/cmdWrite", new PropertyValue(true));
					route.set("/receiving", new PropertyValue(false));
				}
			} else {
				route.set("/op/latitude", new PropertyValue(rp.x));
				route.set("/op/longitude", new PropertyValue(rp.y));
				route.set("/op/altitude", new PropertyValue(rp.z));
				route.set("/cmdOperPoint", new PropertyValue(true));

				MissionAck p = new MissionAck();
				p.target_system = 0;
				p.target_component = 0;
				p.type = Consts.MAV_RESULT_ACCEPTED;
				mavLink.send(new Message(sysId, compId, Id.MISSION_ACK, p.get()));
			}
		}
	}

	private ProtocolDebugAdapter mavLink;
	private final Map<Byte, Handler> handlers;
	private long lastCycleTime;
	
	private final byte sysId = 1;
	private final byte compId = 0;
}
